package simulatie.models;


import javax.media.opengl.GL;
import javax.vecmath.Vector3f;

/**
 * De AGV Klasse
 * @author TetraCON
 *
 */
	public class AGV extends Model {
	private Vector3f bestemming;
	private boolean heeftBestemming = false, bestemmingbereikt = false, idle = true;
	private boolean heeftContainer, clockwise = false;
	private ModelReader modelReader;
	private int eindBestemming = 0;
	private int huidigePositie = 0;
	private int turntrig = 0;
	private Container container;

	
	private float[][] bestemmingen = {{10.8f,0.05f,1.0f}, {10.8f,0.05f,10.0f}, {10.8f,0.05f,30.0f}, {10.8f,0.05f,50.0f}, {10.8f,0.05f,70.0f},
		{10.8f,0.05f,79.39936f}, {13.000009f,0.05f,79.39936f}, {15.000016f,0.05f,79.39936f}, {18.000027f,0.05f,79.39936f}, {20.000034f,0.05f,79.39936f}, {23.000046f,0.05f,79.39936f}, {25.000053f,0.05f,79.39936f}, {28.000065f,0.05f,79.39936f}, {30.000072f,0.05f,79.39936f}, {33.000065f,0.05f,79.39936f}, {35.000034f,0.05f,79.39936f}, {39.399967f,0.05f,79.39936f},
		{39.599964f,0.05f,79.39936f}, {39.599964f,0.05f,76.9994f}, {39.599964f,0.05f,72.99946f}, {39.599964f,0.05f,68.99952f}, {39.599964f,0.05f,64.99958f}, {39.599964f,0.05f,60.99964f}, {39.599964f,0.05f,56.999702f}, {39.599964f,0.05f,52.999763f}, {39.599964f,0.05f,48.999825f},
		{39.599964f,0.05f,38.999977f}, {39.599964f,0.05f,36.90001f}, {39.599964f,0.05f,34.90004f}, {39.599964f,0.05f,32.90007f}, {39.599964f,0.05f,30.90008f}, {39.599964f,0.05f,28.900072f}, {39.599964f,0.05f,26.900064f}, {39.599964f,0.05f,25.000057f}, {39.599964f,0.05f,22.90005f}, {39.599964f,0.05f,21.000042f}, {39.599964f,0.05f,19.000034f}, {39.599964f,0.05f,17.000027f}, {39.599964f,0.05f,15.000019f}, {39.599964f,0.05f,13.000011f}, {39.599964f,0.05f,11.000004f}, {39.599964f,0.05f,8.999996f}, {39.599964f,0.05f,6.999994f}, {39.599964f,0.05f,4.9999957f}, {39.599964f,0.05f,2.9999976f},  
		{39.599964f,0.05f,0.9999982f}, {35.40003f,0.05f,0.9999982f}, {30.900076f,0.05f,0.9999982f}, {26.400059f,0.05f,0.9999982f}, {21.900042f,0.05f,0.9999982f}, {17.400024f,0.05f,0.9999982f}, {12.900007f,0.05f,0.9999982f}};
	
	private float[][] draaipunten = {{10.8f,0.05f,1.0f},{10.8f,0.05f,79.39936f},{39.599964f,0.05f,79.39936f},{39.599964f,0.05f,0.9999982f}};
	
	private float[] startpunt = {10.8f,0.05f,-10.0f};
	private boolean staStil = false;
/**
 * Constructor - plaats de AGV op een standaard positie
 */
	public AGV() { 
		this(new Vector3f(10.8f,0.05f,1.0f));  //Start positie
	}
	
/**
 * Constructor - plaats de AGV op een meegegeven positie
 */	
	public AGV(Vector3f position) {
		super();
		setColor(0, 128, 255);
		this.position = position;
		modelReader = new ModelReader("D:\\Projects\\Java\\Containing\\Containing\\res\\wagen.cpp");
		setFaceIndicies();
		setVertices();
		setNormals();
		setTexCoords();
		setScale(1,1,1);
		//System.out.println("Bestemmingen.length: " + bestemmingen.length);
	}
	
/**
 * geef het model weer met de gewenste eigenschappen
 */
	public void displayModel(GL gl) {
		this.gl = gl;
		if (!idle) {
		if (heeftBestemming && !staStil) {
			beweegAGV();
			//System.out.println("beweegAGV()");
		}
		for (int i = 0; i < draaipunten.length; i++) {
			if (((position.x <= draaipunten[i][0] + 0.01f) && (position.x >= draaipunten[i][0] - 0.01f))
				&& ((position.z <= draaipunten[i][2] + 0.01f) && (position.z >= draaipunten[i][2] - 0.01f))) {
				if (rotations.size() == 0 && turntrig == 1) {
					turntrig = 0;
					addRotation(90,0,0,1);
					//System.out.println("addRotation");
					if (heeftContainer) {
						container.addRotation(90, 0, 0, 1);
					}
				}
				else if (rotations.size() != 0 && turntrig == 1) {
					turntrig = 0;
					removeLastRotation();
					//System.out.println("removeLastRotation");
					if (heeftContainer) {
						container.removeLastRotation();
					}
				}
				else {
					turntrig++;
					//System.out.println("turntrig++");
				}
			}
		}
		if (huidigePositie == eindBestemming  && ! staStil) {
			//heeftBestemming = false;
			if (!heeftContainer) {
				addContainer();
			}
			else {
				removeContainer();
			}
			bestemmingbereikt = true;
		}
		else if (!heeftBestemming){
			if (!clockwise) {
				//System.out.println("huidigePositie: " + huidigePositie);
				//System.out.println("bestemmingenLength-1 : " + (bestemmingen.length-1));
				if (huidigePositie != bestemmingen.length-1) {
					setBestemming(new Vector3f(bestemmingen[huidigePositie+1]));
					//System.out.println("setBestemming: " + (huidigePositie+1));
				}
				else {
					setBestemming(new Vector3f(bestemmingen[0][0], bestemmingen[0][1], bestemmingen[0][2]));
				}
			}
			else {
				if (huidigePositie != 0) {
					setBestemming(new Vector3f(bestemmingen[huidigePositie-1]));
					//System.out.println("setBestemming: " + (huidigePositie-1));
				}
				else {
					setBestemming(new Vector3f(bestemmingen[bestemmingen.length-1][0], bestemmingen[bestemmingen.length-1][1], bestemmingen[bestemmingen.length-1][2]));
				}
			}
		}
		drawVertices();
		if (heeftContainer) {
			container.displayModel(gl);
		}
		}
	}

/**
 * Zet de bestemming van de AGV
 * @param bestemming - de bestemming als Vector3f
 */	
	private void setBestemming (Vector3f bestemming) {
		this.bestemming = bestemming;
		heeftBestemming = true;
	}
	
/**
 * Zet de bestemming van de AGV <br>
 * <br>
 * <i>Deze methode alleen van buiten de klasse aanroepen.</i>
 * @param x - X-positie als float
 * @param y - Y-positie als float
 * @param z - Z-positie als float
 * 
 */
	public void setBestemming (int nr) {
		int pos = huidigePositie;
		//System.out.println(huidigePositie);
		//System.out.println(nr);
		int ccnt = 0;
		while (pos != nr) {
			if (pos == 51) {
				pos = -1;
			}
			//System.out.println("ccnt: " + ccnt);
			//System.out.println("pos: " + pos);
			//System.out.println("nr: " + nr);
			pos++;
			ccnt++;
		}

		pos = huidigePositie;
		int cnt = 0;
		while  (pos != nr) {
			if (pos == 0) {
				pos = bestemmingen.length;		
			}
			pos--;
			//System.out.println("cnt: " + cnt);
			cnt++;
		}
		//System.out.println("AANTAL STAPPEN CCLOCK: " + ccnt);
		//System.out.println(huidigePositie);
		//System.out.println("AANTAL STAPPEN CLOCK: " + cnt);
		//System.out.println(huidigePositie);
		
		if (eindBestemming != nr) {
			if (ccnt < cnt) {
				clockwise = false;
				if (huidigePositie+1 > bestemmingen.length -1) {
					setBestemming(new Vector3f(bestemmingen[0][0], bestemmingen[0][1], bestemmingen[0][2]));
				}
				else {
					setBestemming(new Vector3f(bestemmingen[huidigePositie+1][0], bestemmingen[huidigePositie+1][1], bestemmingen[huidigePositie+1][2]));
				}
			}
			else {
				clockwise = true;
				if (huidigePositie != 0) {
					setBestemming(new Vector3f(bestemmingen[huidigePositie-1][0], bestemmingen[huidigePositie-1][1], bestemmingen[huidigePositie-1][2]));
					//System.out.println("" + bestemmingen[huidigePositie-1][0] + ", " +  bestemmingen[huidigePositie-1][1] + ", " + bestemmingen[huidigePositie-1][2]);
				}
				else {
					setBestemming(new Vector3f(bestemmingen[bestemmingen.length-1][0], bestemmingen[bestemmingen.length-1][1], bestemmingen[bestemmingen.length-1][2]));
				}
			}
			bestemmingbereikt = false;
			idle = false;
			eindBestemming = nr;
			staStil = false;
		}		
	}
	
/**
 * Doe een beweging als een bestemming is ingesteld
 */
	private void beweegAGV () {
		//System.out.println("Eindbestemming: " + eindBestemming);
		//System.out.println("Huidige positie: " + huidigePositie);
		
		if (((position.x <= bestemming.x + 0.01f) && (position.x >= bestemming.x - 0.01f)) 
				&& ((position.z <= bestemming.z + 0.01f) && (position.z >= bestemming.z - 0.01f))) {
			heeftBestemming = false;
			position = bestemming;
			if (!clockwise) {
				if (huidigePositie != bestemmingen.length-1) {
					huidigePositie++;
				}
				else {
					huidigePositie = 0;
				}
			}
			else {
				if (huidigePositie != 0) { 
					huidigePositie--;
				}
				else {
					huidigePositie = bestemmingen.length-1;
				}
			}
			//System.out.println("beweegAGV(): heeftBestemming = false");
		}
		else if (position.x < bestemming.x) {
			setPosition(position.x+0.1f,position.y , position.z);
			if (heeftContainer) {
				container.setPosition(container.getPosition().x + 0.1f, container.getPosition().y, container.getPosition().z);
			}
		}
		else if (position.x > bestemming.x) {
			setPosition(position.x-0.1f,position.y , position.z);
			if (heeftContainer) {
				container.setPosition(container.getPosition().x - 0.1f, container.getPosition().y, container.getPosition().z);
			}
		}
		else if (position.z < bestemming.z) {
			setPosition(position.x, position.y , (0.1f)+position.z);
			if (heeftContainer) {
				container.setPosition(container.getPosition().x, container.getPosition().y, container.getPosition().z + 0.1f);
			}
		}		
		else if (position.z > bestemming.z) {		
			setPosition(position.x,position.y, position.z-0.1f);
			if (heeftContainer) {
				container.setPosition(container.getPosition().x, container.getPosition().y, container.getPosition().z - 0.1f);
			}	
		}
		//System.out.println(position.toString());
		if (heeftContainer) {
			//System.out.println(container.position.toString());
		}	
	}
		
/**
 *  True als de bestemming is bereikt
 */	
	public boolean opBestemming() {
		return bestemmingbereikt;
	}

/**
 * Haal de FaceIndicies uit de modelreader 
 */
	protected void setFaceIndicies() {
		faceIndicies = modelReader.getFaceIndicies();		
	}

/**
 * Haal de Normals uit de modelreader
 */	
	protected void setNormals() {
		normals = modelReader.getNormals();		
	}

/**
 * Haal de TexCoords uit de modelreader
 */	
	protected void setTexCoords() {
		 texCoords = modelReader.getTexCoords();		
	}

/**
 * Haal de vertices uit de modelreader
 */
	protected void setVertices() {
		 vertices = modelReader.getVertices();		
	}
	
/**
 * Zet een container op de AGV
 */
 	public void addContainer() {
 		if (!heeftContainer) {
 			container = new Container();
 			container.setPosition(getPosition().x, getPosition().y + 0.157f, getPosition().z);
 			//System.out.println("Container added: " + container.position.toString());
 			Rotation rot;
 			for (int i = 0; i < rotations.size(); i++) {
 				rot = rotations.get(i);
 				container.addRotation(rot.getAngle(), rot.getRotxyz());
 			}
 			heeftContainer = true;
 			//System.out.println(heeftContainer);
 		}
	}
	
/**
 * Haal de container van de AGV
 */
 	public void removeContainer() {
		container = null;
		heeftContainer = false;
		//System.out.println("Container removed!");
	}
	
	public void idle() {
		idle = true;
	}
	
	public void staStil() {
			staStil = true;
	}
	
	public void init() {
		super.init();
		addRotation(90, 0, 0, 1);
	}

	public int getEindbestemming() {
		return eindBestemming;
	}
	
	public boolean getstaStil() {
		return staStil;
	}
	
	
}

